#ifndef PLANNER_H
#define PLANNER_H

#include "basic_typedef.h"
#include "Scene.h"
#include "Graph.h"
#include "Kd_tree_d.h"
#include <CGAL/Random.h>
#include <boost/shared_ptr.hpp>
#include "Sampler.h"
#include "CollisionDetector.h"
#include "GuiUtils.h"

class Planner
{
public:
    
    ////////////////////////
    // C'tors & D'tors
    ////////////////////////

    Planner(Scene* scene);

    ~Planner(void);

    ////////////////////////
    // modifiers
    ////////////////////////

    void run();

    ////////////////////////
    // helpers
    ////////////////////////

private:
    Point_2        transformToCGAL(QPointF qp)
    {
        // x and y coordinates of the point
        double x = qp.x();
        double y = qp.y();

        Point_2 cpoint(x,y);
        return cpoint;
    }

    void    extract_scenario_from_gui()
    {
        Point_2 ptl = transformToCGAL(m_scene->getRoomTopLeft());
        Point_2 pbr = transformToCGAL(m_scene->getRoomBottomRight());
        Point_2 pbl(ptl.x(), pbr.y());
        Point_2 ptr(pbr.x(), ptl.y());
        m_room = Room(ptl,pbr);

        QVector<QPolygonF> obstacles = m_scene->getObstacles();

        vector<QPolygonF> robots = m_scene->getRobotPolygons();
        foreach(QPolygonF qp, robots)
        {
            Polygon_2 cp;
            for (int j=0; j < qp.size(); j++)
            {
                cp.push_back(transformToCGAL(qp[j]));
            }

            m_robot_polygons.push_back(cp);
        }

        foreach(QPolygonF qp, obstacles)
        {
            Polygon_2 cp;
            for (int j=0; j < qp.size(); j++)
            {
                cp.push_back(transformToCGAL(qp[j]));
            }

            m_obstacles.push_back(cp);
        }

        Polygon_2 wall_left;
        wall_left.push_back(pbl);
        wall_left.push_back(ptl);
    
        Polygon_2 wall_top;
        wall_top.push_back(ptl);
        wall_top.push_back(ptr);

        Polygon_2 wall_right;
        wall_right.push_back(ptr);
        wall_right.push_back(pbr);

        Polygon_2 wall_bot;
        wall_bot.push_back(pbl);
        wall_bot.push_back(pbr);

        m_obstacles.push_back(wall_left);
        m_obstacles.push_back(wall_top);
        m_obstacles.push_back(wall_right);
        m_obstacles.push_back(wall_bot);
    
        QVector<QVector<QPointF> > startPositions = m_scene->getStartPositions();
        QVector<QVector<QPointF> > targetPositions = m_scene->getTargetPositions();

        m_robot_num = targetPositions.size();

        for (int g = 0; g < m_robot_num; g++)
        {
            m_start_confs.push_back(transformToCGAL(startPositions[g][0]));
            m_target_confs.push_back(transformToCGAL(targetPositions[g][0]));
        }
    }

    enum MOVEMENT{
        MATCH,
        UP,
        LEFT
    };

    //TODO: set the correct price
    static const int GAP_EXT_PRICE;

    int getDelta(PointDPtr p, PointDPtr q);
    vector<MPATH> generatePrmPaths(int numOfPaths);
    PointDPtr vectorConfToPointD(vector<Conf> vec);
    vector<vector<PointDPtr>> getPotentialBridgeEdges(MPATH p, MPATH q);
    bool localPlanner(PointDPtr from, PointDPtr to);
  

   
    ///////////////////////
    // Data members
    /////////////////////// 

    Scene*                      m_scene;
    vector<Polygon_2>           m_robot_polygons;
    Obstacles                   m_obstacles;            // polygonal obstacles + scenario boundaries
    vector<Conf>                m_start_confs, m_target_confs;
    Room                        m_room;
    int                         m_robot_num;
    MPATH                       m_path;                 // path section - robot - configuration

    Graph<PointDPtr>*               m_hgraph;                // Graph structure
    map<PointDPtr, vector<Conf>> m_pointdptr_to_point2;  //Maps between a pointer to point_d to point in the configuration space
   /* Kd_tree_d<Kernel_d>*        m_kd_tree;              // Kd-tree for nearest neighbor search

    vector<vector<Conf>>        m_vertices;             // Contains all the points in the configuration space
    map<vector<Conf>, Point_d>  m_expandMap;            // Maps between a point in the configuration space to the 2d dimention equivelent
    map<Point_d, LABEL, cmpPoint_d>         m_map;                  // Maps between any point to it's index

    vector<CollisionDetector*>	m_cols;                 // Collision detectors for each robot with obstacles
    vector<Obstacles>           m_robotObs;             // The other robots as obstacles
    vector<vector<CollisionDetector*>> m_colsRobots;    // Collision detectors for each robot with other robots
    vector<Sampler*>			m_samplers;             // Samplers for each robot*/
};

#endif